#include "ms575_algbacks1s2rectedge.h"
using namespace MeasurE575;

AlgBackS1S2RectEdge::AlgBackS1S2RectEdge(int i):AlgBase(i)
{

}

void AlgBackS1S2RectEdge::operator()(cv::Mat &srcImg,
                                     int id,
                                     std::map<string, float> &measRes,
                                     int fixtureId)
{
        //back S1-> 25, S2-> 26

      std::vector<float> dists3,dists4;
      std::vector<vector<Point2f> > contourres3,contourres4;
      std::vector<int> paras3,paras4;
      for(int j=0;j< m_dataAllVec[25].m_edgeParam.cols;j++)
      {
          paras3.push_back(m_dataAllVec[25].m_edgeParam.at<int>(0,j));
          paras4.push_back(m_dataAllVec[26].m_edgeParam.at<int>(1,j));
      }
      omp_set_num_threads(2);
      std::vector<cv::Rect> rangS1Vec,rangS2Vec;
#pragma omp parallel sections
    {
        #pragma omp section
        {
             TemplateMatch(srcImg,m_dataAllVec[25],rangS1Vec);
             edgedetect obj;
             obj.NewGetRectEdge(srcImg(rangS1Vec[0]),paras3,dists3,contourres3);

             for(auto &pt1:contourres3)
             {
                 for(auto &pt2:pt1)
                 {
                     pt2.x+=rangS1Vec[0].x;
                     pt2.y+=rangS1Vec[0].y;
                 }
             }
        }
        #pragma omp section
        {
              TemplateMatch(srcImg,m_dataAllVec[26],rangS2Vec);
              edgedetect obj;
              obj.NewGetRectEdge(srcImg(rangS2Vec[0]),paras4,dists4,contourres4);

              for(auto &pt1:contourres4)
              {
                  for(auto &pt2:pt1)
                  {
                      pt2.x+=rangS2Vec[0].x;
                      pt2.y+=rangS2Vec[0].y;
                  }
              }
        }

    }
      std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
      while (!AlgBase::flagVec[id])
      {
          (*AlgBase::cvarptrVec[id]).wait(lock);
      }

#pragma omp parallel sections
    {
        #pragma omp section
        {
           for(auto &pt1:contourres3)
           {
              for(auto &pt2:pt1)
              {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
              }
           }
         }
        #pragma omp section
          {
              for(auto &pt1:contourres4)
              {
                  for(auto &pt2:pt1)
                  {
                      cv::Point2f ptsOut;
                      CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                      pt2=ptsOut;
                  }
              }
          }
     }


    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    edgedetect obj;

    float linedist=obj.GetLineDist(contourres3[0], contourres3[1],1);
    measRes.insert(std::make_pair(string("FAI27-S1-X-distance_pixel"), linedist));
    float linedistreal = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI27-S1-X-distance")], linedist);
    measRes.insert(std::make_pair(string("FAI27-S1-X-distance_real"), linedistreal));

    float lineDist1 = fabs(contourres3[0][0].x - contourres3[1][0].x);
    measRes.insert(std::make_pair(string("FAI27-S1-X1-distance_pixel"), lineDist1));
    float lineDistReal1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI27-S1-X1-distance")], linedist);
    measRes.insert(std::make_pair(string("FAI27-S1-X1-distance_real"), lineDistReal1));

    float lineDist2 = fabs(contourres3[0][1].x - contourres3[1][1].x);
    measRes.insert(std::make_pair(string("FAI27-S1-X2-distance_pixel"), lineDist2));
    float lineDistReal2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI27-S1-X2-distance")], lineDist2);
    measRes.insert(std::make_pair(string("FAI27-S1-X2-distance_real"), lineDistReal2));

    float lineDist3 = fabs(contourres3[0][2].x - contourres3[1][2].x);
    measRes.insert(std::make_pair(string("FAI27-S1-X3-distance_pixel"), lineDist3));
    float lineDistReal3 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI27-S1-X3-distance")], lineDist3);
    measRes.insert(std::make_pair(string("FAI27-S1-X3-distance_real"), lineDistReal3));

    float linedisty=obj.GetLineDist(contourres3[3], contourres3[4], 2);
    measRes.insert(std::make_pair(string("FAI29-S1-Y-distance_pixel"), linedisty));
    float linedistyreal = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI29-S1-Y-distance")], linedisty);
    measRes.insert(std::make_pair(string("FAI29-S1-Y-distance_real"), linedistyreal));


    float lineDist4 = fabs(contourres3[3][0].y - contourres3[4][0].y);
    measRes.insert(std::make_pair(string("FAI29-S1-Y1-distance_pixel"), lineDist4));
    float lineDistReal4 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI29-S1-Y1-distance")], lineDist4);
    measRes.insert(std::make_pair(string("FAI29-S1-Y1-distance_real"), lineDistReal4));

    float lineDist5 = fabs(contourres3[3][1].y - contourres3[4][1].y);
    measRes.insert(std::make_pair(string("FAI29-S1-Y2-distance_pixel"), lineDist5));
    float lineDistReal5 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI29-S1-Y2-distance")], lineDist5);
    measRes.insert(std::make_pair(string("FAI29-S1-Y2-distance_real"), lineDistReal5));

    float lineDist6 = fabs(contourres3[3][2].y - contourres3[4][2].y);
    measRes.insert(std::make_pair(string("FAI29-S1-Y3-distance_pixel"), lineDist6));
    float lineDistReal6 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI29-S1-Y3-distance")], lineDist6);
    measRes.insert(std::make_pair(string("FAI29-S1-Y3-distance_real"), lineDistReal6));


    float fai44para=obj.GetParallelism(contourres3[2],1);
    measRes.insert(std::make_pair(string("FAI44-S1-X-parallelism_pixel"), fai44para));
    float fai44 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI44-S1-X-parallelism")], fai44para);
    measRes.insert(std::make_pair(string("FAI44-S1-X-parallelism_real"), fai44));

    float fai45para=obj.GetParallelism(contourres3[5],2);
    measRes.insert(std::make_pair(string("FAI45-S1-Y-parallelism_pixel"), fai45para));
    float fai45 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI45-S1-Y-parallelism")], fai45para);
    measRes.insert(std::make_pair(string("FAI45-S1-Y-parallelism_real"), fai45));

    measRes.insert(std::make_pair(string("FAI101-S1-Y1-distance_pixel"), dists3[1]));
    float fai101_1 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI101-S1-Y1-distance")], dists3[1]);
    measRes.insert(std::make_pair(string("FAI101-S1-Y1-distance_real"), fai101_1));

    measRes.insert(std::make_pair(string("FAI101-S1-Y2-distance_pixel"), dists3[0]));
    float fai101_2 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI101-S1-Y2-distance")], dists3[0]);
    measRes.insert(std::make_pair(string("FAI101-S1-Y2-distance_real"), fai101_2));

    float midlinedist=obj.GetLine2LineMid(contourres3[3],contourres3[4],2);
    measRes.insert(std::make_pair(string("FAI198-S1-Y-distance_pixel"), midlinedist));
    float midlinedistreal = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI198-S1-Y-distance")], midlinedist);
    measRes.insert(std::make_pair(string("FAI198-S1-Y-distance_real"), midlinedistreal));
    float midlinedistx=obj.GetLine2LineMid(contourres3[0],contourres3[1],1);
    measRes.insert(std::make_pair(string("FAI227-S1-X-distance_pixel"), midlinedistx));
    float midlinedistxreal = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI227-S1-X-distance")], midlinedistx);
    measRes.insert(std::make_pair(string("FAI227-S1-X-distance_real"), midlinedistxreal));

    float linedist2=obj.GetLineDist(contourres4[0],contourres4[1],2);
    measRes.insert(std::make_pair(string("FAI200-S2-Y-distance_pixel"), linedist2));
    float linedist2real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI200-S2-Y-distance")], linedist2);
    measRes.insert(std::make_pair(string("FAI200-S2-Y-distance_real"), linedist2real));
    float linedist3=obj.GetLineDist(contourres4[2],contourres4[3],1);
    measRes.insert(std::make_pair(string("FAI201-S2-X-distance_pixel"), linedist3));
    float linedist3real = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI201-S2-X-distance")], linedist3);
    measRes.insert(std::make_pair(string("FAI201-S2-X-distance_real"), linedist3real));
}

